#include "EmbStdLib_Malloc.h"
#include "EmbStdLib_Queue.h"

#define ENABLE_LOGGING (1) // Enable logging
#define ENABLE_MALLOC (1)  // Enable dynamic memory allocation
#define ENABLE_MAIN (0)    // Enable main function example

// Define global static arrays to store messages for each queue
#if !ENABLE_MALLOC
uint8 queue1_data[MAX_QUEUE1_MESSAGES][MAX_QUEUE1_MESSAGE_LENGTH] = {0x00};
uint8 queue2_data[MAX_QUEUE2_MESSAGES][MAX_QUEUE2_MESSAGE_LENGTH] = {0x00};
#endif

#if ENABLE_LOGGING
#define LOG(queue, level, format, ...) logMessage(queue, level, format, ##__VA_ARGS__)

// Get the current time as a string
uint8 *currentTime()
{
  time_t now = time(NULL_PTR);
  uint8 *time_str = ctime(&now);
  time_str[strlen(time_str) - 1] = '\0'; // Remove newline character
  return time_str;
}

// Log message and print to console
void logMessage(Queue *queue, LogLevel level, const uint8 *format, ...)
{
  if (queue->log_enabled && level >= queue->log_level)
  {
    va_list args;
    va_start(args, format);
    const uint8 *level_str;
    switch (level)
    {
    case LOG_LEVEL_DEBUG:
      level_str = "DEBUG";
      break;
    case LOG_LEVEL_INFO:
      level_str = "INFO";
      break;
    case LOG_LEVEL_WARN:
      level_str = "WARN";
      break;
    case LOG_LEVEL_ERROR:
      level_str = "ERROR";
      break;
    default:
      level_str = "UNKNOWN";
      break;
    }

    // Generate log message
    uint8 log_buffer[256];
    vsnprintf(log_buffer, sizeof(log_buffer), format, args);
    va_end(args);

    // Output to console
    printf("[%s] [%s] %s\n", currentTime(), level_str, log_buffer);
  }
}
#else
#define LOG(queue, level, format, ...)
#endif

uint8 QueueInit(boolean log_enabled, LogLevel log_level, Queue *queue, uint32 max_messages, uint32 message_length, uint8 *data)
{
  if (!queue)
  {
    return QUEUE_ERROR_MEMORY;
  }
#if ENABLE_MALLOC
  queue->data = (uint8 *)memory_malloc(max_messages * message_length * sizeof(uint8));
  if (!queue->data)
  {
    return QUEUE_ERROR_MEMORY;
  }
#else
  queue->data = data;
#endif
  queue->max_messages = max_messages;
  queue->message_length = message_length;
  queue->current_memory = 0;
  queue->front = 0;
  queue->rear = 0;
  queue->size = 0;
  queue->log_enabled = log_enabled;
  queue->log_level = log_level;
  queue->log_error = QUEUE_SUCCESS;
  return QUEUE_SUCCESS;
}

void destroyQueue(Queue *queue)
{
#if ENABLE_MALLOC
  if (queue->data)
  {
    memory_free(queue->data);
    queue->data = NULL_PTR;
  }
#else
  memset(queue->data, 0x00, queue->max_messages * queue->message_length * sizeof(uint8));
  queue->data = NULL_PTR;
#endif
  queue->max_messages = 0;
  queue->message_length = 0;
  queue->current_memory = 0;
  queue->front = 0;
  queue->rear = 0;
  queue->size = 0;
  queue->log_enabled = FALSE;
  queue->log_level = LOG_LEVEL_INFO;
  queue->log_error = QUEUE_SUCCESS;

  LOG(queue, LOG_LEVEL_INFO, "Queue destroyed");
}

uint32 putQueue(Queue *queue, const uint8 *message, uint32 message_length)
{
  if (message_length > queue->message_length)
  {
    LOG(queue, LOG_LEVEL_ERROR, "Message length exceeds the limit");
    return QUEUE_ERROR_SIZE;
  }

  if (queue->size >= queue->max_messages)
  {
    LOG(queue, LOG_LEVEL_WARN, "Queue is full, cannot putQueue message.");
    return QUEUE_ERROR_FULL;
  }

  memcpy(&queue->data[queue->rear * queue->message_length], message, message_length);
  queue->rear = (queue->rear + 1) % queue->max_messages;
  queue->size++;
  queue->current_memory += queue->message_length;

  LOG(queue, LOG_LEVEL_INFO, "putQueue message of size %u.", message_length);
  return QUEUE_SUCCESS;
}

uint32 getQueue(Queue *queue, uint8 *message, uint32 message_length)
{
  if (queue->size == 0)
  {
    LOG(queue, LOG_LEVEL_WARN, "Queue is empty, cannot getQueue message.");
    return QUEUE_ERROR_EMPTY;
  }

  if (message_length < queue->message_length)
  {
    LOG(queue, LOG_LEVEL_ERROR, "Message length is too small to getQueue message.");
    return QUEUE_ERROR_SIZE;
  }

  memcpy(message, &queue->data[queue->front * queue->message_length], queue->message_length);
  memset(&queue->data[queue->front * queue->message_length], 0x00, queue->message_length);
  queue->front = (queue->front + 1) % queue->max_messages;
  queue->size--;
  queue->current_memory -= queue->message_length;

  LOG(queue, LOG_LEVEL_INFO, "getQueue message of size %u.", queue->message_length);
  return QUEUE_SUCCESS;
}

void printQueue(Queue *queue)
{
#if ENABLE_LOGGING
  printf("Queue (size: %u, current_memory: %u):\n", queue->size, queue->current_memory);
  for (uint32 i = 0; i < queue->size; i++)
  {
    uint32 index = (queue->front + i) % queue->max_messages;
    printf("  Message %u: ", i + 1);
    for (uint32 j = 0; j < queue->message_length; j++)
    {
      printf("%02X ", queue->data[index * queue->message_length + j]);
    }
    printf("\n");
  }
#endif
}

#if ENABLE_MAIN
uint32 main()
{
  // Create multiple queues
  Queue queue_instance1;
  Queue *queue1 = &queue_instance1;
  Queue queue_instance2;
  Queue *queue2 = &queue_instance2;

  // Initialize queues and specify global static arrays if dynamic memory allocation is disabled
  uint8 queue1_ret = QueueInit(TRUE, LOG_LEVEL_DEBUG, queue1, MAX_QUEUE1_MESSAGES, MAX_QUEUE1_MESSAGE_LENGTH,
#if !ENABLE_MALLOC
                               (uint8 *)queue1_data
#else
                               NULL_PTR
#endif
  );
  uint8 queue2_ret = QueueInit(TRUE, LOG_LEVEL_DEBUG, queue2, MAX_QUEUE2_MESSAGES, MAX_QUEUE2_MESSAGE_LENGTH,
#if !ENABLE_MALLOC
                               (uint8 *)queue2_data
#else
                               NULL_PTR
#endif
  );

  if (queue1_ret || queue2_ret)
  {
    printf("Failed to create queues\n");
    return QUEUE_ERROR_MEMORY;
  }

  // Messages for queue1
  const uint8 messages1_1[8] = {0x01, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x00};
  const uint8 messages1_2[8] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x00};
  const uint8 messages1_3[8] = {0x01, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x00};
  const uint8 messages1_4[8] = {0x01, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x00};

  // Messages for queue2
  const uint8 messages2_1[16] = {0x02, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};
  const uint8 messages2_2[16] = {0x02, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};
  const uint8 messages2_3[16] = {0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};
  const uint8 messages2_4[16] = {0x02, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};

  // Add messages to queue1
  if (putQueue(queue1, messages1_1, sizeof(messages1_1)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages1_1 in queue1\n");
  }
  if (putQueue(queue1, messages1_2, sizeof(messages1_2)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages1_2 in queue1\n");
  }
  if (putQueue(queue1, messages1_3, sizeof(messages1_3)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages1_3 in queue1\n");
  }
  if (putQueue(queue1, messages1_4, sizeof(messages1_4)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages1_4 in queue1\n");
  }

  // Add messages to queue2
  if (putQueue(queue2, messages2_1, sizeof(messages2_1)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages2_1 in queue2\n");
  }
  if (putQueue(queue2, messages2_2, sizeof(messages2_2)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages2_2 in queue2\n");
  }
  if (putQueue(queue2, messages2_3, sizeof(messages2_3)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages2_3 in queue2\n");
  }
  if (putQueue(queue2, messages2_4, sizeof(messages2_4)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages2_4 in queue2\n");
  }

  // Print the contents of both queues
  printf("Queue 1:\n");
  printQueue(queue1);

  printf("Queue 2:\n");
  printQueue(queue2);

  // Read both queues and print remaining contents
  uint8 buffer[16];
  for (uint32 i = 0; i < 4; i++)
  {
    if (getQueue(queue1, buffer, sizeof(buffer)) == QUEUE_SUCCESS)
    {
      printf("getQueued message (queue1): ");
      for (uint32 j = 0; j < 8; j++)
      {
        printf("%02X ", buffer[j]);
      }
      printf("\n");
    }
    else
    {
      printf("Failed to getQueue\n");
    }
  }
  printQueue(queue1);

  for (uint32 i = 0; i < 4; i++)
  {
    if (getQueue(queue2, buffer, sizeof(buffer)) == QUEUE_SUCCESS)
    {
      printf("getQueued message (queue2): ");
      for (uint32 j = 0; j < 16; j++)
      {
        printf("%02X ", buffer[j]);
      }
      printf("\n");
    }
    else
    {
      printf("Failed to getQueue\n");
    }
  }
  printQueue(queue2);

  // Destroy queues
  destroyQueue(queue1);
  destroyQueue(queue2);

  return 0;
}
#endif
